#include "bsp_uart.h"

#include <stdio.h>
#include <stdlib.h>
#include <string.h>
#include "freertos/FreeRTOS.h"
#include "freertos/task.h"
#include "driver/uart.h"
#include "esp_log.h"

#include "config.h"
#include "fifo.h"
#include "mpc.h"
#include "commu.h"

#ifndef USE_SYS_MEM
#include "m_memory.h"
extern uint8_t memory_pool[16 * 1024];
extern MemoryPoolManager pool;
#endif

static const char *TAG = "bsp_uart";

#define BSP_UART UART_NUM_1
#define BUF_SIZE (256)
#define BSP_UART_STACK_SIZE (4 * 1024)
#define BSP_UART_TX_QUEUE_SIZE 32

static uint8_t buf[BUF_SIZE];
static QueueHandle_t bsp_uart_queue;
static QueueHandle_t bsp_uart_tx_queue;
fifo_buffer_t bsp_uart_rx_fifo;

int SendDataToUart(uint8_t *buf, uint8_t len)
{
#ifdef USE_SYS_MEM
    commu_msg_t *msg = (commu_msg_t*)malloc(sizeof(commu_msg_t));
#else
    commu_msg_t *msg = (commu_msg_t*)mempool_alloc(&pool, sizeof(commu_msg_t));
#endif
    if (msg == NULL) {
        return 1;
    }
    msg->len = len;
#ifdef USE_SYS_MEM
    msg->buf = (uint8_t *)malloc(len);
#else
    msg->buf = (uint8_t*)mempool_alloc(&pool, len);
#endif
    if (msg->buf == NULL) {
#ifdef USE_SYS_MEM
    free(msg);
#else
    mempool_free(&pool, msg);
#endif
        return 1;
    }
    memcpy(msg->buf, buf, len);
    if (xQueueSend(bsp_uart_tx_queue, &msg, portMAX_DELAY) != pdTRUE) {
        ESP_LOGE(TAG, "queue send fail");
        return 2;
    }
    return 0;
}

static void uart_event_task(void *pvParameters)
{
    uart_event_t event;
    for (;;) {
        //Waiting for UART event.
        if (xQueueReceive(bsp_uart_queue, (void *)&event, (TickType_t)portMAX_DELAY)) {
            switch (event.type) {
                case UART_DATA:
                    int len = uart_read_bytes(BSP_UART, buf, event.size, portMAX_DELAY);
                    if (len > 0) {
                        
                        fifo_write_buf(&bsp_uart_rx_fifo, buf, len); // 写入 FIFO                
                        while (1) { // 尝试解析完整帧
                            uint8_t temp_hdr[6]; // 先Peek至少前6字节判断帧头和长度
                            uint8_t avail = fifo_peek(&bsp_uart_rx_fifo, temp_hdr, sizeof(temp_hdr));
                            if (avail < 6) break;
                            if (temp_hdr[0] != HEADER1 || temp_hdr[1] != HEADER2) {
                                fifo_consume(&bsp_uart_rx_fifo, 1);  // 头不对，跳过一字节
                                continue;
                            }
                            uint8_t expected_len = temp_hdr[5];  // data length
                            size_t packet_size = PACKET_TOTAL_SIZE(expected_len);
                            if (fifo_available(&bsp_uart_rx_fifo) < packet_size) {
                                ESP_LOGI(TAG, "data len is error");
                                break;  // 数据未收全
                            }
                            // 可以读取整个包了
                            // uint8_t *packet_buf = malloc(packet_size);
                            uint8_t send_buf[packet_size];
                            // uint8_t *packet_buf = (uint8_t *)mempool_alloc(&pool, packet_size);
                            // if (!packet_buf) continue;
                            fifo_read_buf(&bsp_uart_rx_fifo, send_buf, packet_size);
                            // ESP_LOG_BUFFER_HEX(TAG, packet_buf, packet_size);
                            SendToCommu((uint8_t *)send_buf, packet_size);
                            // free(packet_buf);
                            // mempool_free(&pool, packet_buf);
                        }
                    }
                    break;
                case UART_FIFO_OVF:
                    ESP_LOGI(TAG, "hw fifo overflow");
                    uart_flush_input(BSP_UART);
                    xQueueReset(bsp_uart_queue);
                    break;
                case UART_BUFFER_FULL:
                    ESP_LOGI(TAG, "ring buffer full");
                    uart_flush_input(BSP_UART);
                    xQueueReset(bsp_uart_queue);
                    break;
                case UART_BREAK:
                    ESP_LOGE(TAG, "uart rx break");
                    break;
                case UART_PARITY_ERR:
                    ESP_LOGE(TAG, "uart parity error");
                    break;
                case UART_FRAME_ERR:
                    ESP_LOGE(TAG, "uart frame error");
                    break;
                // UART_PATTERN_DET
                // case UART_PATTERN_DET:
                //     uart_get_buffered_data_len(BSP_UART, &buffered_size);
                //     int pos = uart_pattern_pop_pos(BSP_UART);
                //     ESP_LOGI(TAG, "[UART PATTERN DETECTED] pos: %d, buffered size: %d", pos, buffered_size);
                //     if (pos == -1) {
                //         // There used to be a UART_PATTERN_DET event, but the pattern position queue is full so that it can not
                //         // record the position. We should set a larger queue size.
                //         // As an example, we directly flush the rx buffer here.
                //         uart_flush_input(BSP_UART);
                //     } else {
                //         uart_read_bytes(BSP_UART, dtmp, pos, 100 / portTICK_PERIOD_MS);
                //         uint8_t pat[PATTERN_CHR_NUM + 1];
                //         memset(pat, 0, sizeof(pat));
                //         uart_read_bytes(BSP_UART, pat, PATTERN_CHR_NUM, 100 / portTICK_PERIOD_MS);
                //         ESP_LOGI(TAG, "read data: %s", dtmp);
                //         ESP_LOGI(TAG, "read pat : %s", pat);
                //     }
                //     break;
                //Others
                default:
                    ESP_LOGI(TAG, "uart event type: %d", event.type);
                    break;
            }
        }
    }
    vTaskDelete(NULL);
}

static void uart_tx_task(void *pvParameters)
{
    commu_msg_t *msg;
    for (;;) {
        //Waiting for UART event.
        if (xQueueReceive(bsp_uart_tx_queue, (void *)&msg, (TickType_t)portMAX_DELAY) == pdTRUE) {
            uart_write_bytes(BSP_UART, msg->buf, msg->len);
#ifdef USE_SYS_MEM
            free(msg->buf);
            free(msg);
#else
            mempool_free(&pool, msg->buf);
            mempool_free(&pool, msg);
#endif
        }
    }
    vTaskDelete(NULL);
}

void bsp_uart_init(void)
{
    ESP_LOGI(TAG, "init");
    uart_config_t uart_config = {
        .baud_rate = 3000000,
        .data_bits = UART_DATA_8_BITS,
        .parity = UART_PARITY_DISABLE,
        .stop_bits = UART_STOP_BITS_1,
        .flow_ctrl = UART_HW_FLOWCTRL_DISABLE,
        .source_clk = UART_SCLK_DEFAULT,
    };
    uart_driver_install(BSP_UART, BUF_SIZE * 2, BUF_SIZE * 2, 20, &bsp_uart_queue, 0);
    uart_param_config(BSP_UART, &uart_config);
    uart_set_pin(BSP_UART, 14, 13, UART_PIN_NO_CHANGE, UART_PIN_NO_CHANGE);
    uart_pattern_queue_reset(BSP_UART, 20);
    xTaskCreate(uart_event_task, "bsp_uart_rx", BSP_UART_STACK_SIZE, NULL, 12, NULL);
    bsp_uart_tx_queue = xQueueCreate(BSP_UART_TX_QUEUE_SIZE, sizeof(commu_msg_t));
    if (bsp_uart_tx_queue == NULL) {
        ESP_LOGE(TAG, "queue init fail");
    }
    fifo_init(&bsp_uart_rx_fifo);
    xTaskCreate(uart_tx_task, "bsp_uart_tx", BSP_UART_STACK_SIZE, NULL, 13, NULL);
}